#include "LPC17xx.h"
#include "lpc17xx_uart.h"
#include "lpc17xx_pinsel.h"
#include "hapr_assmt_linefollow.h"

#define UART_SEND_B(s,n)   UART_Send((LPC_UART_TypeDef *)LPC_UART3, s, n, BLOCKING)
#define UART_SEND_NB(s,n)   UART_Send((LPC_UART_TypeDef *)LPC_UART3, s, n, NONE_BLOCKING)
#define UART_RCV_B(s,n)   UART_Receive((LPC_UART_TypeDef *)LPC_UART3, s, n, BLOCKING)
#define UART_RCV_NB(s,n)   UART_Receive((LPC_UART_TypeDef *)LPC_UART3, s, n, NONE_BLOCKING)

static calib_status cs = RAW; //Maintain state for calibration of line sensors
static uint8_t data[10]; //Buffer for sensor readings

//Preprocessor define masks requirement to supply m3pi commands by pointer to data region
static uint8_t command_list[6] = {0x86, 0x87, 0xB5, 0xBA, 0xBB, 0xBC, };
#define CMD_RAWS       (command_list)
#define CMD_CALS       (command_list+1)
#define CMD_RESET      (command_list+2)
#define CMD_AUTO       (command_list+3)
#define CMD_STARTPID   (command_list+4)
#define CMD_STOPPID    (command_list+5)

void LINE_SerialInit(void){
	UART_CFG_Type UARTConfigStruct;
	UART_FIFO_CFG_Type UARTFIFOConfigStruct;
	PINSEL_CFG_Type PinCfg;

	//Config UART3 connection for messages to m3pi
	PinCfg.Funcnum = 2;
	PinCfg.OpenDrain = 0;
	PinCfg.Pinmode = 0;
	PinCfg.Portnum = 0;
	PinCfg.Pinnum = 0;
	PINSEL_ConfigPin(&PinCfg);
	PinCfg.Pinnum = 1;
	PINSEL_ConfigPin(&PinCfg);

      	UART_ConfigStructInit(&UARTConfigStruct);
	UARTConfigStruct.Baud_rate = (uint32_t) 115200; //Required by m3pi spec

	UART_FIFOConfigStructInit(&UARTFIFOConfigStruct);
	UART_Init((LPC_UART_TypeDef *)LPC_UART3, &UARTConfigStruct);
	UART_FIFOConfig((LPC_UART_TypeDef *)LPC_UART3, &UARTFIFOConfigStruct);
	UART_TxCmd((LPC_UART_TypeDef *)LPC_UART3, ENABLE);
}

void LINE_ResetCalibration(void){
  UART_SEND_B(CMD_RESET, 1);
  cs = RAW;
}

void LINE_AutoCalibrate(void){
  UART_SEND_B(CMD_AUTO, 1);
  uint8_t rcv = 0;
  UART_RCV_B(&rcv, 1); //Wait on Calibration to complete
  cs = CALIBRATED;
}

void LINE_GetSensors(SensorVals* sv){
  UART_SEND_B((cs?CMD_CALS:CMD_RAWS),1); //m3pi message depends on sensor calibration
  UART_RCV_B(data, 10);

  sv->status = cs;
  sv->PC0 = ((uint16_t)data[1] << 8) + data[0]; //glue the bytes together in the struct
  sv->PC1 = ((uint16_t)data[3] << 8) + data[2];
  sv->PC2 = ((uint16_t)data[5] << 8) + data[4];
  sv->PC3 = ((uint16_t)data[7] << 8) + data[6];
  sv->PC4 = ((uint16_t)data[9] << 8) + data[8];
}

void LINE_StartFollow(LineFollowCfg* fc){


  uint8_t cmd_string[6] = { *CMD_STARTPID, //Deference as actual byte value is required
			    fc->FollowSpeed,
			    fc->P_Factor,
			    fc->P_Quotient,
			    fc->D_Factor,
			    fc->D_Quotient, };

    UART_SEND_B(cmd_string, 6);
}

void LINE_EndFollow(uint8_t spd){
  UART_SEND_B(CMD_STOPPID,1);
  uint8_t buf[4] = {0xC1, spd, 0xC5, spd};
  UART_SEND_B(buf, 4);  
}
